/*
 * @Author: laladuduqq
 * @Date: 2024-12-24 19:29:09
 * @LastEditors: laladuduqq
 * @LastEditTime: 2025-01-19 19:40:49
 */
#include "LQR.h"
#include "arm_math.h"
#include "board_com.h"
#include "damiao.h"
#include "dji.h"
#include "imu.h"
#include "message.h"
#include "motor_def.h"
#include "offline.h"
#include "robotdef.h"
#include "rttypes.h"
#include "signal_create.h"
#include "user_lib.h"
#include "vcom.h"

#define LOG_TAG              "gimcmd"
#define LOG_LVL              LOG_LVL_DBG
#include <ulog.h>

#if defined (SENTRY_MODE)
static Publisher_t *gimbal_pub;                   // 云台应用消息发布者(云台反馈给cmd)
static Subscriber_t *gimbal_sub;                  // cmd控制消息订阅者
static Gimbal_Upload_Data_s gimbal_feedback_data; // 回传给cmd的云台状态信息
static Gimbal_Ctrl_Cmd_s gimbal_cmd_recv;         // 来自cmd的控制信息
static Gimbal_Ctrl_Cmd_s small_gimbal_cmd_recv;
DJIMotor_t *yaw_motor ={0};
DMMOTOR_t *pitch_motor ={0};

// 正弦运动参数
static SinusoidalMotionParams_t pitch_linear;
static SinusoidalMotionParams_t yaw_linear;
static float current_yaw = 0.0f;
static float current_pitch = 0.0f;

static rt_tick_t yaw_at_limit_time = 0;
static rt_tick_t pitch_at_limit_time = 0;
static rt_tick_t current_time;

void gimbal_init(void)
{
    // YAW
    
    Motor_Init_Config_s yaw_config = {
        .offline_manage_motor ={
            .event = OFFLINE_GIMBAL_YAW,
            .enable = OFFLINE_ENABLE,
            .online_state = STATE_OFFLINE,
            .error_level =OFFLINE_ERROR_LEVEL,
            .beep_times = 1,
            .last_time =rt_tick_get(),
            .offline_time =500,
        },
        .can_init_config = {
            .can_handle = &can1_bus,
            .tx_id = 1,
        },
    #ifdef SENTRY_BOARD_BIG_YAW
        .controller_param_init_config = {
            .other_angle_feedback_ptr = &INS.YawTotalAngle,
            .other_speed_feedback_ptr = &INS.Gyro[2],
            .lqr_config ={
                .K ={22.3607f,2.8714f},
                .output_max = 2.223,
                .output_min =-2.223,
                .state_dim = 2,
                .compensation_type =COMPENSATION_NONE,
            }
        },
        .controller_setting_init_config = {
            .control_algorithm = CONTROL_LQR,
            .feedback_reverse_flag =FEEDBACK_DIRECTION_NORMAL,
            .angle_feedback_source =OTHER_FEED,
            .speed_feedback_source =OTHER_FEED,
            .outer_loop_type = ANGLE_LOOP,
            .close_loop_type = ANGLE_LOOP | SPEED_LOOP,
            .Feedback_Object_Type =MOTOR_ROTOR_FEEDBACK,
        },
        .motor_type = GM6020_CURRENT,
    #endif
    #ifdef SENTRY_BOARD_SMALL_YAW_1
        .controller_param_init_config = {
            .other_angle_feedback_ptr = &INS.YawTotalAngle,
            .other_speed_feedback_ptr = &INS.Gyro[2],
            .lqr_config ={ 
                .K ={2.5f,0.211f},     //motor .K ={2.5f,0.211f},   //gyro .K ={5.472f,0.8999f},
                .output_max = 2.23,
                .output_min =-2.23,
                .state_dim = 2,
                .compensation_type = COMPENSATION_NONE,
                .ki = 3,
                .integral_max = 0.5,
            }
        },
        .controller_setting_init_config = {
            .control_algorithm = CONTROL_LQR,
            .feedback_reverse_flag =FEEDBACK_DIRECTION_NORMAL,
            .angle_feedback_source =MOTOR_FEED,
            .speed_feedback_source =MOTOR_FEED,
            .outer_loop_type = ANGLE_LOOP,
            .close_loop_type = ANGLE_LOOP | SPEED_LOOP,
            .Feedback_Object_Type =MOTOR_ROTOR_FEEDBACK,
        },
        .motor_type = GM6020_CURRENT,
    #endif
    #ifdef SENTRY_BOARD_SMALL_YAW_2
        .controller_param_init_config = {
            .other_angle_feedback_ptr = &INS.YawTotalAngle,
            .other_speed_feedback_ptr = &INS.Gyro[2],
            .lqr_config ={
                .K ={3.7f,0.222f},
                .output_max = 2.23,
                .output_min =-2.23,
                .state_dim = 2,
                .compensation_type = COMPENSATION_NONE,
            }
        },
        .controller_setting_init_config = {
            .control_algorithm = CONTROL_LQR,
            .feedback_reverse_flag =FEEDBACK_DIRECTION_NORMAL,
            .angle_feedback_source =MOTOR_FEED,
            .speed_feedback_source =MOTOR_FEED,
            .outer_loop_type = ANGLE_LOOP,
            .close_loop_type = ANGLE_LOOP | SPEED_LOOP,
            .Feedback_Object_Type =MOTOR_ROTOR_FEEDBACK,
        },
        .motor_type = GM6020_CURRENT,
    #endif
    };
    yaw_motor = DJIMotorInit(&yaw_config);
    // PITCH
    Motor_Init_Config_s pitch_config = {
        .offline_manage_motor ={
            .event = OFFLINE_GIMBAL_PITCH,
            .enable = OFFLINE_ENABLE,
            .online_state = STATE_OFFLINE,
            .error_level =OFFLINE_ERROR_LEVEL,
            .beep_times = 2,
            .last_time =rt_tick_get(),
            .offline_time =1000,
        },
        .can_init_config = {
            .can_handle = &can1_bus,
            .tx_id = 0X23,
            .rx_id = 0X206,
        },
        .controller_param_init_config = {
            .other_angle_feedback_ptr = &INS.Pitch,
            .other_speed_feedback_ptr = &INS.Gyro[0],
            .lqr_config ={
                .K ={44.7214f,3.3411f}, //28.7312f,2.5974f
                .output_max = 7,
                .output_min = -7,
                .state_dim = 2,
                .compensation_type =COMPENSATION_GRAVITY,
                .arm_length = 0.09,
                .gravity_force = 16,
            }
        },
        .controller_setting_init_config = {
            .control_algorithm = CONTROL_LQR,
            .feedback_reverse_flag =FEEDBACK_DIRECTION_REVERSE,
            .angle_feedback_source =OTHER_FEED,
            .speed_feedback_source =OTHER_FEED,
        },
        .motor_type = DM4310};
    #ifdef SENTRY_BOARD_SMALL_YAW_1
        pitch_motor = DMMotorInit(&pitch_config,MIT_MODE);
        gimbal_sub = SubRegister("gimbal_cmd", sizeof(Gimbal_Ctrl_Cmd_s));

        // 初始化正弦运动参数
        pitch_linear.amplitude = (SMALL_YAW_1_PITCH_MAX_ANGLE - SMALL_YAW_1_PITCH_MIN_ANGLE)/2.0f;
        pitch_linear.frequency = 1.0f; // 0.1 Hz
        pitch_linear.offset = SMALL_YAW_1_PITCH_MIN_ANGLE;
        pitch_linear.phase = 0.0f;

        yaw_linear.amplitude = (SMALL_YAW_1_MAX_ANGLE - SMALL_YAW_1_MIN_ANGLE)/2.0f;
        yaw_linear.frequency = 0.1f; // 0.1 Hz
        yaw_linear.offset = (SMALL_YAW_1_MAX_ANGLE + SMALL_YAW_1_MIN_ANGLE)/2.0f;
        yaw_linear.phase = 0.0f;

        current_yaw = SMALL_YAW_1_ALIGN_ANGLE;
        current_pitch = SMALL_YAW_1_PITCH_HORIZON_ANGLE;
    #endif 
    #ifdef SENTRY_BOARD_SMALL_YAW_2
        pitch_motor = DMMotorInit(&pitch_config,MIT_MODE);
        gimbal_sub = SubRegister("gimbal_cmd", sizeof(Gimbal_Ctrl_Cmd_s));

        // 初始化正弦运动参数
        pitch_linear.amplitude = (SMALL_YAW_2_PITCH_MAX_ANGLE - SMALL_YAW_2_PITCH_MIN_ANGLE)/2.0f;
        pitch_linear.frequency = 1.0f; // 0.1 Hz
        pitch_linear.offset = SMALL_YAW_2_PITCH_MIN_ANGLE;
        pitch_linear.phase = 0.0f;
        
        yaw_linear.amplitude = (SMALL_YAW_2_MAX_ANGLE - SMALL_YAW_2_MIN_ANGLE)/2.0f;
        yaw_linear.frequency = 0.1f; // 0.1 Hz
        yaw_linear.offset = (SMALL_YAW_2_MAX_ANGLE + SMALL_YAW_2_MIN_ANGLE)/2.0f;
        yaw_linear.phase = 0.0f;
        
        current_yaw = SMALL_YAW_2_ALIGN_ANGLE;
        current_pitch = SMALL_YAW_2_PITCH_HORIZON_ANGLE;
    #endif 
    
    #ifdef SENTRY_BOARD_BIG_YAW
    gimbal_pub = PubRegister("gimbal_feed", sizeof(Gimbal_Upload_Data_s));
    gimbal_sub = SubRegister("gimbal_cmd", sizeof(Gimbal_Ctrl_Cmd_s));
    #endif 
}

static void gimbal_big_yaw()
{
    SubGetMessage(gimbal_sub, &gimbal_cmd_recv);
    if (offline_manage[SYSTEM_PROTECT].online_state==STATE_ONLINE)
    {
        
        if (gimbal_cmd_recv.gimbal_mode == GIMBAL_ZERO_FORCE)
        {
            DJIMotorStop(yaw_motor);
        }
        else if (gimbal_cmd_recv.gimbal_mode == GIMBAL_GYRO_MODE || gimbal_cmd_recv.gimbal_mode == GIMBAL_KEEPING)
        {
            DJIMotorEnable(yaw_motor);
            DJIMotorSetRef(yaw_motor,gimbal_cmd_recv.yaw); // yaw和pitch会在robot_cmd中处理好多圈和单圈
        }
        else if (gimbal_cmd_recv.gimbal_mode == GIMBAL_AUTO_MODE) {
            DJIMotorEnable(yaw_motor);
            DJIMotorSetRef(yaw_motor,0); 
        }
        else
        {
            DJIMotorStop(yaw_motor);
        }
    }
    else
    { 
        DJIMotorStop(yaw_motor);
    }
    // 设置反馈数据,主要是imu和yaw的ecd
    if (offline_manage[OFFLINE_GIMBAL_YAW].online_state ==STATE_ONLINE)
    {
        gimbal_feedback_data.yaw_motor_single_round_angle = yaw_motor->measure.angle_single_round;
        // 推送消息
        PubPushMessage(gimbal_pub, (void *)&gimbal_feedback_data);
    }
}

static void gimbal_small_yaw_1()
{
    SubGetMessage(gimbal_sub, &small_gimbal_cmd_recv);
    if (offline_manage[SYSTEM_PROTECT].online_state==STATE_ONLINE)
    {        
        switch (small_gimbal_cmd_recv.gimbal_mode) 
        {
            case GIMBAL_AUTO_MODE:
            {
                DMMotorEnable(pitch_motor);
                DJIMotorEnable(yaw_motor);
                if (offline_manage[OFFLINE_MINIPC_HEART].online_state == STATE_ONLINE) 
                {
                    if (Vision_Recv.distance==-1) 
                    { 
                        // 进行正弦运动
                        float target_yaw, target_pitch;
                        perform_sinusoidal_motion(&yaw_linear, &pitch_linear, &current_yaw, &current_pitch, &target_yaw, &target_pitch);
                        VAL_LIMIT(target_yaw, SMALL_YAW_1_MIN_ANGLE, SMALL_YAW_1_MAX_ANGLE);
                        VAL_LIMIT(target_pitch, SMALL_YAW_1_PITCH_MIN_ANGLE, SMALL_YAW_1_PITCH_MAX_ANGLE);
                        DMMotorSetRef(pitch_motor, target_pitch);
                        DJIMotorSetRef(yaw_motor, target_yaw);
                    }
                    else 
                    {
                        float tmpyaw = Vision_Recv.yaw;
                        float tmppitch = Vision_Recv.pitch;
                        Vision_Recv.fire_advice =0;
                        VAL_LIMIT(tmpyaw, SMALL_YAW_1_MIN_ANGLE, SMALL_YAW_1_MAX_ANGLE);
                        VAL_LIMIT(tmppitch, SMALL_YAW_1_PITCH_MIN_ANGLE, SMALL_YAW_1_PITCH_MAX_ANGLE);
        
                        DMMotorSetRef(pitch_motor, tmppitch);  
                        DJIMotorSetRef(yaw_motor, tmpyaw);

                        // 更新 current_yaw 和 current_pitch
                        current_yaw = tmpyaw;
                        current_pitch = tmppitch;
        
                    }
                }
                else 
                {
                    DMMotorSetRef(pitch_motor, SMALL_YAW_1_PITCH_HORIZON_ANGLE);  
                    DJIMotorSetRef(yaw_motor, SMALL_YAW_1_ALIGN_ANGLE);
                                       
                    // 更新 current_yaw 和 current_pitch
                    current_yaw = SMALL_YAW_1_ALIGN_ANGLE;
                    current_pitch = SMALL_YAW_1_PITCH_HORIZON_ANGLE;
                }
                break;
            }
            case GIMBAL_KEEPING_SMALL_YAW_2:
                DMMotorEnable(pitch_motor);
                DJIMotorEnable(yaw_motor);
                DMMotorSetRef(pitch_motor, small_gimbal_cmd_recv.pitch);  
                DJIMotorSetRef(yaw_motor, small_gimbal_cmd_recv.yaw);
                break;
            case GIMBAL_KEEPING:
            case GIMBAL_KEEPING_SMALL_YAW_1:
                DJIMotorEnable(yaw_motor);
                DJIMotorSetRef(yaw_motor, SMALL_YAW_1_ALIGN_ANGLE);
                DMMotorEnable(pitch_motor);
                DMMotorSetRef(pitch_motor,SMALL_YAW_1_PITCH_HORIZON_ANGLE);
                break;
            case GIMBAL_ZERO_FORCE:
                DJIMotorStop(yaw_motor);
                DMMotorStop(pitch_motor);
            default:
                break;
        }
    }else{ DJIMotorStop(yaw_motor);DMMotorStop(pitch_motor);}
}

static void gimbal_small_yaw_2()
{
    SubGetMessage(gimbal_sub, &small_gimbal_cmd_recv);
    if (offline_manage[SYSTEM_PROTECT].online_state==STATE_ONLINE)
    {
        switch (small_gimbal_cmd_recv.gimbal_mode) 
        {
            case GIMBAL_AUTO_MODE:
            {
                DMMotorEnable(pitch_motor);
                DJIMotorEnable(yaw_motor);
                if (offline_manage[OFFLINE_MINIPC_HEART].online_state == STATE_ONLINE) 
                {
                    if (Vision_Recv.distance==-1) 
                    { 
                        float target_yaw, target_pitch;
                        perform_sinusoidal_motion(&yaw_linear, &pitch_linear, &current_yaw, &current_pitch, &target_yaw, &target_pitch);
                        
                        VAL_LIMIT(target_yaw, SMALL_YAW_2_MIN_ANGLE, SMALL_YAW_2_MAX_ANGLE);
                        VAL_LIMIT(target_pitch, SMALL_YAW_2_PITCH_MIN_ANGLE, SMALL_YAW_2_PITCH_MAX_ANGLE);
                        DMMotorSetRef(pitch_motor, target_pitch);
                        DJIMotorSetRef(yaw_motor, target_yaw);
                    }
                    else 
                    {
                        float tmpyaw = Vision_Recv.yaw;
                        float tmppitch = Vision_Recv.pitch;
                        VAL_LIMIT(tmpyaw, SMALL_YAW_2_MIN_ANGLE, SMALL_YAW_2_MAX_ANGLE);
                        VAL_LIMIT(tmppitch, SMALL_YAW_2_PITCH_MIN_ANGLE, SMALL_YAW_2_PITCH_MAX_ANGLE);
        
                        DMMotorSetRef(pitch_motor, tmppitch);  
                        DJIMotorSetRef(yaw_motor, tmpyaw);

                        // 更新 current_yaw 和 current_pitch
                        current_yaw = tmpyaw;
                        current_pitch = tmppitch;
        
                    }
                }
                else 
                {
                    DMMotorSetRef(pitch_motor, SMALL_YAW_2_PITCH_HORIZON_ANGLE);  
                    DJIMotorSetRef(yaw_motor, SMALL_YAW_2_ALIGN_ANGLE);

                    // 更新 current_yaw 和 current_pitch
                    current_yaw = SMALL_YAW_2_ALIGN_ANGLE;
                    current_pitch = SMALL_YAW_2_PITCH_HORIZON_ANGLE;
                }
                break;
            }
            case GIMBAL_KEEPING_SMALL_YAW_1:
                DMMotorEnable(pitch_motor);
                DJIMotorEnable(yaw_motor);
                DMMotorSetRef(pitch_motor, small_gimbal_cmd_recv.pitch);  
                DJIMotorSetRef(yaw_motor, small_gimbal_cmd_recv.yaw);
                break;
            case GIMBAL_KEEPING:
            case GIMBAL_KEEPING_SMALL_YAW_2:
                DJIMotorEnable(yaw_motor);
                DJIMotorSetRef(yaw_motor, SMALL_YAW_2_ALIGN_ANGLE);
                DMMotorEnable(pitch_motor);
                DMMotorSetRef(pitch_motor,SMALL_YAW_2_PITCH_HORIZON_ANGLE);
                break;
            case GIMBAL_ZERO_FORCE:
                DJIMotorStop(yaw_motor);
                DMMotorStop(pitch_motor);
            default:
                break;
        }
    }else{ DJIMotorStop(yaw_motor);DMMotorStop(pitch_motor);}
}

static void gimbal_thread_entry(void *parameter)
{   
    rt_thread_mdelay(2000);
    gimbal_init();
    rt_uint8_t send_counter = 0; // 计数器用于控制发送频率
    while (1)
    {
    #ifdef SENTRY_BOARD_SMALL_YAW_1
        gimbal_small_yaw_1();
        if (send_counter % 2 == 0) 
        {
            Vision_Send.header =0X5A;
            Vision_Send.mode =0;
            Vision_Send.roll =INS.Roll;
            Vision_Send.pitch =-INS.Pitch;
            Vision_Send.yaw =yaw_motor->measure.total_angle;
            Vision_Send.end = 0X5D;
            vision_send_process(&Vision_Send);
            send_counter =0;
        }
        send_counter++;
    #endif 
    #ifdef SENTRY_BOARD_SMALL_YAW_2
        gimbal_small_yaw_2();
        if (send_counter % 2 == 0) 
        {
            Vision_Send.header =0X5A;
            Vision_Send.mode =0;
            Vision_Send.roll =INS.Roll;
            Vision_Send.pitch =-INS.Pitch;
            Vision_Send.yaw =yaw_motor->measure.total_angle;
            Vision_Send.end = 0X5D;
            vision_send_process(&Vision_Send);
            send_counter =0;
        }
        send_counter++;
    #endif 
    #ifdef SENTRY_BOARD_BIG_YAW
        gimbal_big_yaw();
        if (send_counter %2 == 0) 
        {
            Nav_Send.header = 0x5A;
            Nav_Send.current_hp_percent = board_com.Chassis_Upload_Data.current_hp_percent;
            Nav_Send.game_progress = 1;
            Nav_Send.event_center_aera_rmul = board_com.Chassis_Upload_Data.event_center_aera_rmul;
            Nav_Send.event_supply_area_rmul = board_com.Chassis_Upload_Data.event_supply_area_rmul;
            Nav_Send.event_supply_area_rmuc = board_com.Chassis_Upload_Data.event_supply_area_rmuc;
            Nav_Send.end = 0x5D ;
            nav_send_process(&Nav_Send);
            send_counter =0;
        }
        send_counter++;
    #endif 
        rt_thread_mdelay(2);
    }

}
int gimbal_thread_entry_init(void)
{
    rt_err_t ret = RT_EOK;
    rt_thread_t thread = rt_thread_create("gimbal", gimbal_thread_entry, RT_NULL, 4096, 30, 10);
    /* 创建成功则启动线程 */
    if (thread != RT_NULL)
    {
        rt_thread_startup(thread);
    }
    else
    {
        ret = RT_ERROR;
    }

    LOG_D("gimbal thread init success!\n");

    return ret;
}

#if defined (SENTRY_BOARD_BIG_YAW) || defined (SENTRY_BOARD_SMALL_YAW_1) || defined (SENTRY_BOARD_SMALL_YAW_2)
INIT_APP_EXPORT(gimbal_thread_entry_init);
#endif 

#else

static Publisher_t *gimbal_pub;                   // 云台应用消息发布者(云台反馈给cmd)
static Subscriber_t *gimbal_sub;                  // cmd控制消息订阅者
static Gimbal_Upload_Data_s gimbal_feedback_data; // 回传给cmd的云台状态信息
static Gimbal_Ctrl_Cmd_s gimbal_cmd_recv;         // 来自cmd的控制信息
DJIMotor_t *yaw_motor ={0};
DMMOTOR_t *pitch_motor ={0};

void gimbal_init(void)
{
    // YAW
    Motor_Init_Config_s yaw_config = {
        .offline_manage_motor ={
            .event = OFFLINE_GIMBAL_YAW,
            .enable = OFFLINE_ENABLE,
            .online_state = STATE_OFFLINE,
            .error_level =OFFLINE_ERROR_LEVEL,
            .beep_times = 1,
            .last_time =rt_tick_get(),
            .offline_time =1000,
        },
        .can_init_config = {
            .can_handle = &can1_bus,
            .tx_id = 1,
        },
        .controller_param_init_config = {
            .angle_PID = {
                .Kp = 5, // 8
                .Ki = 0,
                .Kd = 2,
                .DeadBand = 0.1,
                .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
                .IntegralLimit = 200,
                .MaxOut = 1500,
            },
            .speed_PID = {
                .Kp = 50,  // 50
                .Ki = 200, // 200
                .Kd = 0,
                .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
                .IntegralLimit = 3000,
                .MaxOut = 30000,
            },
            .other_angle_feedback_ptr = &INS.YawTotalAngle,
            .other_speed_feedback_ptr = &INS.Gyro[2],
        },
        .controller_setting_init_config = {
            .angle_feedback_source = OTHER_FEED,
            .speed_feedback_source = OTHER_FEED,
            .outer_loop_type = ANGLE_LOOP,
            .close_loop_type = ANGLE_LOOP | SPEED_LOOP,
            .motor_reverse_flag = MOTOR_DIRECTION_NORMAL,
        },
        .motor_type = GM6020};
    yaw_motor = DJIMotorInit(&yaw_config);
    // PITCH
    Motor_Init_Config_s pitch_config = {
        .offline_manage_motor ={
            .event = OFFLINE_GIMBAL_PITCH,
            .enable = OFFLINE_ENABLE,
            .online_state = STATE_OFFLINE,
            .error_level =OFFLINE_ERROR_LEVEL,
            .beep_times = 2,
            .last_time =rt_tick_get(),
            .offline_time =1000,
        },
        .can_init_config = {
            .can_handle = &can1_bus,
            .tx_id = 0X23,
            .rx_id = 0X206,
        },
        .controller_param_init_config = {
            .angle_PID = {
                .Kp = 0.01, // 8
                .Ki = 0,
                .Kd = 0,
                .DeadBand = 0.1,
                .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit,
                .IntegralLimit = 0,
                .MaxOut = 100,
            },
            .speed_PID = {
                .Kp = 0.3,  // 50
                .Ki = 0, // 200
                .Kd = 0,
                .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit,
                .IntegralLimit = 100,
                .MaxOut = 0.2,
            },
            .other_angle_feedback_ptr = &INS.Pitch,
            .other_speed_feedback_ptr = &INS.Gyro[0],
        },
        .controller_setting_init_config = {
            .angle_feedback_source = MOTOR_FEED,
            .speed_feedback_source = MOTOR_FEED,
            .outer_loop_type = ANGLE_LOOP,
            .close_loop_type = ANGLE_LOOP | SPEED_LOOP,
            .motor_reverse_flag = MOTOR_DIRECTION_NORMAL,
        },
        .motor_type = DM4310};
    pitch_motor = DMMotorInit(&pitch_config,MIT_MODE);
    
    gimbal_pub = PubRegister("gimbal_feed", sizeof(Gimbal_Upload_Data_s));
    gimbal_sub = SubRegister("gimbal_cmd", sizeof(Gimbal_Ctrl_Cmd_s));

}

static void gimbal_thread_entry(void *parameter)
{   
    rt_thread_mdelay(2000);
    gimbal_init();
    while (1)
    {
        if (offline_manage[SYSTEM_PROTECT].online_state==STATE_ONLINE)
        {
            SubGetMessage(gimbal_sub, &gimbal_cmd_recv);
            if (gimbal_cmd_recv.gimbal_mode == GIMBAL_ZERO_FORCE)
            {
                DJIMotorStop(yaw_motor);
                DMMotorStop(pitch_motor);
            }
            else if (gimbal_cmd_recv.gimbal_mode == GIMBAL_GYRO_MODE)
            {
                DJIMotorEnable(yaw_motor);
                DMMotorEnable(pitch_motor);
                DJIMotorSetRef(yaw_motor, gimbal_cmd_recv.yaw); // yaw和pitch会在robot_cmd中处理好多圈和单圈
                DMMotorSetRef(pitch_motor, gimbal_cmd_recv.pitch);
            }
        }else{ DJIMotorStop(yaw_motor);DMMotorStop(pitch_motor);}
        // 设置反馈数据,主要是imu和yaw的ecd
        if (offline_manage[OFFLINE_GIMBAL_YAW].online_state ==STATE_ONLINE)
        {
            gimbal_feedback_data.yaw_motor_single_round_angle = yaw_motor->measure.angle_single_round;
            // 推送消息
            PubPushMessage(gimbal_pub, (void *)&gimbal_feedback_data);
        }
        rt_thread_mdelay(5);
    }

}
int gimbal_thread_entry_init(void)
{
    rt_err_t ret = RT_EOK;
    rt_thread_t thread = rt_thread_create("gimbal", gimbal_thread_entry, RT_NULL, 4096, 30, 10);
    /* 创建成功则启动线程 */
    if (thread != RT_NULL)
    {
        rt_thread_startup(thread);
    }
    else
    {
        ret = RT_ERROR;
    }

    LOG_D("gimbal thread init success!\n");

    return ret;
}

INIT_APP_EXPORT(gimbal_thread_entry_init);

#endif